/*
 * Filename: c:\Users\jy_Huang\Desktop\test\test\MDK-ARM\mpu6500.c
 * Path: c:\Users\jy_Huang\Desktop\test\test\MDK-ARM
 * Created Date: Thursday, March 18th 2021, 6:35:18 pm
 * Author: jjyy-Huang
 * 
 * Copyright (c) 2021 Your Company
 */

#include "mpu6500.h"
#include "stm32f1xx.h"
#include <string.h>

#define MPU_SPI hspi1

#define MPU_ENABLE      HAL_GPIO_WritePin(CS_6500_GPIO_Port, CS_6500_Pin, GPIO_PIN_RESET);
#define MPU_DISABLE     HAL_GPIO_WritePin(CS_6500_GPIO_Port, CS_6500_Pin, GPIO_PIN_SET);

int16_t tmp;
static uint8_t tx, rx;
uint8_t mpu_data_buf[14];
Mpu_data_t mpu_data;

//include accel and gyro  and temperature data
/**
  * @brief  send a byte to specific address register of mpu
  *         
  * @param  address and data to write : 
	*
  * @retval none
  */
void mpu_spi_write_reg(uint8_t addr, uint8_t data){
	MPU_ENABLE;
	tx	= addr & 0x7f;
	HAL_SPI_TransmitReceive(&MPU_SPI, &tx, &rx, 1, 100);
	tx  = data;
	HAL_SPI_TransmitReceive(&MPU_SPI, &tx, &rx, 1, 100);
	MPU_DISABLE;
}

void mpu_read_regs(uint8_t firstAddr, uint8_t *pBuff, uint8_t length){
	MPU_ENABLE;
	
	tx = firstAddr | 0x80;
	HAL_SPI_TransmitReceive(&MPU_SPI, &tx, &rx, 1, 0x05);
	HAL_SPI_Receive(&MPU_SPI, pBuff, length, 0x05);
	MPU_DISABLE;
}

void mpu_read_raw(){
	mpu_read_regs(MPU6500_ACCEL_XOUT_H, mpu_data_buf, 14);
	
	mpu_data.ax = 		mpu_data_buf[0]<<8 | mpu_data_buf[1];
	mpu_data.ay = 		mpu_data_buf[2]<<8 | mpu_data_buf[3];
	mpu_data.az = 		mpu_data_buf[4]<<8 | mpu_data_buf[5];
	
	mpu_data.gx = 	mpu_data_buf[8]<<8  | mpu_data_buf[9];
	mpu_data.gy = 	mpu_data_buf[10]<<8 | mpu_data_buf[11];
	mpu_data.gz = 	mpu_data_buf[12]<<8 | mpu_data_buf[13];
	
	mpu_data.temperature = 	mpu_data_buf[6]<<8 | mpu_data_buf[7];
}

#define MPU6500_InitRegNum 8
void mpu6500_init(void)
{
	uint8_t MPU6500_Init_Data[MPU6500_InitRegNum][2] = {
		{0x80, MPU6500_PWR_MGMT_1},     // Reset Device
		{0x01, MPU6500_PWR_MGMT_1},     // Clock Source
		{0x00, MPU6500_PWR_MGMT_2},     // Enable Acc & Gyro
		{0x06, MPU6500_CONFIG},         // LPS_5Hz
		{0x18, MPU6500_GYRO_CONFIG},    // +-2000dps
//		{0x10, MPU6500_ACCEL_CONFIG},   // +-8G
		{0x00, MPU6500_ACCEL_CONFIG},
//		{0x0b, MPU6500_ACCEL_CONFIG_2}, // Set Acc Data Rates
		{0x06, MPU6500_ACCEL_CONFIG_2}, // Set Acc Data Rates
//		{0x30, MPU6500_INT_PIN_CFG},    // 
//		{0x40, MPU6500_I2C_MST_CTRL},   // I2C Speed 348 kHz
		{0x30, MPU6500_USER_CTRL},      // Enable AUX
	};
	for(uint8_t i=0; i<MPU6500_InitRegNum; i++)
	{
		mpu_spi_write_reg(MPU6500_Init_Data[i][1], MPU6500_Init_Data[i][0]);
		HAL_Delay(5);
  }
	HAL_Delay(500);
}
uint8_t MPU_SPI_Check(void)//检查mpu6500通信是否正常
{
	uint8_t rxbuf;
	mpu_read_regs(0x75,&rxbuf,1);
	return rxbuf;
}

//98D3:36:F59C02


